﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using WorldConqueror.Libraries.Maths.Enumerations;
using WorldConqueror.Libraries.Maths.Factories;
using WorldConqueror.Libraries.Maths.MathShapes;

namespace WorldConqueror.Libraries.Maths.Functions.Intersection
{
    public interface ISphericalPolylinesIntersection : ITwoParamsFunction<IMathSphericalPolyline, IMathSphericalPolyline, IList<IMathCoordinate>>
    {
    }

    public class SphericalPolylinesIntersection : ISphericalPolylinesIntersection
    {
        internal IFunctionsFactory FunctionsFactory
        {
            get;
            private set;
        }

        internal IShapesFactory ShapesFactory
        {
            get;
            private set;
        }

        public SphericalPolylinesIntersection(
            IMathSphericalPolyline parameter1, 
            IMathSphericalPolyline parameter2,
            IFunctionsFactory functionsFactory,
            IShapesFactory shapesFactory)
        {
            Parameter1 = parameter1;
            Parameter2 = parameter2;
            FunctionsFactory = functionsFactory;
            ShapesFactory = shapesFactory;
        }



        public IMathSphericalPolyline Parameter1
        {
            get;
            private set;
        }

        public IMathSphericalPolyline Parameter2
        {
            get;
            private set;
        }

        public IList<IMathCoordinate> Calculate()
        {
            var intersections = new List<IMathCoordinate>();

            for (int firstParameterIndex = 0; firstParameterIndex < Parameter1.MathCoordinates.Count - 1; firstParameterIndex++)
            {
                for (int secondParameterIndex = 0; secondParameterIndex < Parameter2.MathCoordinates.Count - 1; secondParameterIndex++)
                {
                    var intersection = SimpleLineIntersection(Parameter1, firstParameterIndex, Parameter2, secondParameterIndex);

                    if (intersection != null)
                    {
                        intersections.Add(intersection);
                    }
                }
            }

            return intersections;
        }

        public IMathCoordinate SimpleLineIntersection(IMathSphericalPolyline parameter1, int firstParameterStartIndex, IMathSphericalPolyline parameter2, int secondParameterStartIndex)
        {
            var firstPoint = parameter1.MathCoordinates[firstParameterStartIndex];
            var secondPoint = parameter2.MathCoordinates[secondParameterStartIndex];

            var bearingFunction = FunctionsFactory.CreateTwoParametersFunction<double>(FunctionType.Bearing, firstPoint, parameter1.MathCoordinates[firstParameterStartIndex + 1]);
            var firstPointBearing = bearingFunction.Calculate();

            bearingFunction = FunctionsFactory.CreateTwoParametersFunction<double>(FunctionType.Bearing, secondPoint, parameter2.MathCoordinates[secondParameterStartIndex + 1]);
            var secondPointBearing= bearingFunction.Calculate();

            //d12 = 2.asin( √(sin²(Δφ/2) + cos(φ1).cos(φ2).sin²(Δλ/2)) )
            var d12 = 2*Math.Asin(Math.Sqrt(
                Math.Pow(Math.Sin((secondPoint.Latitude - firstPoint.Latitude)/2),2) +
                Math.Cos(firstPoint.Latitude)*Math.Cos(secondPoint.Latitude)*
                Math.Pow(Math.Sin((secondPoint.Longitude - firstPoint.Longitude)/2),2)));

            //φ1 = acos( sin(φ2) − sin(φ1).cos(d12) / sin(d12).cos(φ1) )
            var angle1 =  Math.Acos( Math.Sin( secondPoint.Latitude) - Math.Sin(firstPoint.Latitude)*
                Math.Cos(d12) / Math.Sin(d12) *Math.Cos(firstPoint.Latitude) );
            //φ2 = acos( sin(φ1) − sin(φ2).cos(d12) / sin(d12).cos(φ2) )
            var angle2 =  Math.Acos( Math.Sin( firstPoint.Latitude) - Math.Sin(secondPoint.Latitude)*
                Math.Cos(d12) / Math.Sin(d12) *Math.Cos(secondPoint.Latitude) );

            double angle12;
            double angle21;

            //if sin(λ2−λ1) > 0
            //    θ12 = φ1, θ21 = 2.π − φ2
            //else
            //    θ12 = 2.π − φ1, θ21 = φ2
            if(Math.Sin(secondPoint.Longitude - firstPoint.Longitude) > 0)
            {
                angle12 = angle1;
                angle21 = Math.PI*2 - angle2;
            }
            else
            {
                angle12 = Math.PI*2 - angle1;
                angle21 = angle2;
            }

            //α1 = (θ1 − θ12 + π) % 2.π − π
            //α2 = (θ21 − θ2 + π) % 2.π − π
            var alfa1 = (firstPointBearing - angle12 + Math.PI) % 2 * Math.PI - Math.PI;
            var alfa2 = (angle21 - secondPointBearing + Math.PI) % 2 * Math.PI - Math.PI;

            //α3 = acos( −cos(α1).cos(α2) + sin(α1).sin(α2).cos(d12) )
            //d13 = atan2( sin(d12).sin(α1).sin(α2), cos(α2)+cos(α1).cos(α3) )
            var alfa3 = Math.Acos(-1*Math.Cos(alfa1)*Math.Cos(alfa2) + Math.Sin(alfa1)*Math.Sin(alfa2)*Math.Cos(d12));
            var d13 = Math.Atan2(Math.Sin(d12)*Math.Sin(alfa1)*Math.Sin(alfa2), Math.Cos(alfa2) + Math.Cos(alfa1)*Math.Cos(alfa3));

            //φ3 = asin( sin(φ1).cos(d13) + cos(φ1).sin(d13).cos(θ1) )
            //Δλ13 = atan2( sin(θ1).sin(d13).cos(φ1), cos(d13)−sin(φ1).sin(φ3) ) 
            //λ3 = (λ1+Δλ13+π) % 2.π − π
            var intersectionLatitude = Math.Asin(Math.Sin(angle1)*Math.Cos(d13) + Math.Cos(angle1)*Math.Sin(d13)*Math.Cos(firstPointBearing));
            var angularDifference12 = Math.Atan2(Math.Sin(firstPointBearing)*Math.Sin(d13)*Math.Cos(angle1), Math.Cos(d13) - Math.Sin(angle1)*Math.Sin(intersectionLatitude));
            var intersectionLongitude = (firstPoint.Longitude + angularDifference12 + Math.PI) % 2*Math.PI - Math.PI;
            
            var possibleMathCoordinate = ShapesFactory.CreateCoordinate(intersectionLatitude, intersectionLongitude);

            return possibleMathCoordinate;
        }
    }
}
